#ifndef _MPCSOLVER_H_
#define _MPCSOLVER_H_

#include <iostream>
#include <string>
#include "all.h"
#include <qpOASES.hpp>

// using namespace qpOASES; // contain class "Matrix",ambigurous with Eigen;
using namespace std;
using namespace Eigen;

class MPCsolver
{
private:
    qpOASES::QProblem *qproblem;

    Matrix<double, 13 * PREDICTION_HORIZON, 1> D_;                                 // D(13h*1)
    Matrix<double, 13, 1> x_0_;                                                    // x_0(13*1)
    DiagonalMatrix<double, 13 * PREDICTION_HORIZON, 13 * PREDICTION_HORIZON> Q_;   // Q(13h*13h),  NOTE: Q_ is diagonal matrix
    DiagonalMatrix<double, 12 * PREDICTION_HORIZON, 12 * PREDICTION_HORIZON> R_;   // R(12h*12h),  NOTE: R_ is diagonal matrix
    Matrix<double, 20 * PREDICTION_HORIZON, 12 * PREDICTION_HORIZON, RowMajor> C_; // C(20h*12h),  NOTE: row major storage
    Matrix<double, 20 * PREDICTION_HORIZON, 1> c_lb_;                              // clb(20h*1)
    Matrix<double, 20 * PREDICTION_HORIZON, 1> c_ub_;                              // cub(20h*1)

    Matrix<double, 13 * PREDICTION_HORIZON, 13> A_qp_;                             // Aqp(13h*13)
    Matrix<double, 13 * PREDICTION_HORIZON, 12 * PREDICTION_HORIZON> B_qp_;        // Bqp(13h*12h)
    Matrix<double, 12 * PREDICTION_HORIZON, 12 * PREDICTION_HORIZON, RowMajor> H_; // H(12h*12h),  NOTE: row major storage
    Matrix<double, 12 * PREDICTION_HORIZON, 1> g_;                                 // g(13h*1)
    Matrix<double, 12 * PREDICTION_HORIZON, 1> U_;                                 // U(12h*1)

public:
    MPCsolver(/* args */);
    ~MPCsolver();

    void setD_and_x0(const Matrix<double, 13 * PREDICTION_HORIZON, 1> &vecD,
                     const Matrix<double, 13, 1> &x_0);

    // 计算A_qp_、B_qp_、H_、g_、C_
    void calculateAll(const Matrix<double, PREDICTION_HORIZON, 1> &psi_k_d,
                      const Matrix<double, 3 * PREDICTION_HORIZON, 4> &r_k_ix);

    void qpSolveU_first();
    void qpSolveU_next(Matrix<double, 12,1> &U_optimal);

};

#endif
